/**
 * movingObjectTest.cpp
 *
 * @author Pieter Nys <pieter-nys@hotmail.com>
 * @date    01/03/11
 */
#include "movingObject.h"
#include <iostream>
using namespace std;

int main() {
    
    //defining a first waypoint
    MovingObject mo1("Jos"),mo2("Jef");
    WayPoint wp1, wp2;


    wp1.setX(5);
    wp1.setY(6);
    wp1.setTheta(3);
    wp1.setTimeStamp(1);
    wp1.setKick(false);

    wp2.setX(6);
    wp2.setY(7);
    wp2.setTheta(3);
    wp2.setTimeStamp(2);
    wp2.setKick(false);

    mo1.setWp(wp1);
    mo1.setWp(wp2);

    mo1.getWpNext(3.0).Properties();

    cout << "------------------------------------------------------" << endl;

    cout << "Start Test bereken snijpunt" << endl;

    WayPoint wp10(0,0,0,0),wp11(1,1,1,1);
    WayPoint wp20(2,0,0,0),wp21(2,1,0,1);


    mo1.setWp(wp10);
    mo1.setWp(wp20);

    mo2.setWp(wp20);
    mo2.setWp(wp21);

    WayPoint intersect = mo1.getWpIntersection(mo2);


    cout << "Intersection of mo1 and mo2" << endl;

    intersect.Properties();

    cout << "------------------------------------------------------" << endl;

    cout << "Get IDs" << endl << endl;

    cout << mo1.getID() << " en " << mo2.getID() << endl;
    
    return 0;
}
